www.gusucode.com > Robotics Playground工具箱matlab源码 > Robotics Playground/lib/Resources/Utils/PlaygroundCreateSimMap.m
function mapForSim = PlaygroundCreateSimMap(Arena,blk) % This function parses through the settings in the obstacle environment % mask and creates an occupancy grid in order to implement sensors % Copyright 2018 The MathWorks, Inc. arenaDimensions=[Arena.width Arena.length]; % Normalize the arena dimensions normDims=arenaDimensions/max(arenaDimensions); mapScale=200/max(arenaDimensions); mapGridSize=round(normDims*200); % Initialize obstacle map locObsMap = ones(mapGridSize(1),mapGridSize(2),'single'); for idx = 1:size(Arena.obj,2) % add walls to occupancy map locObsMap(1:5,:)=0; locObsMap(end-5:end,:)=0; locObsMap(:,1:5)=0; locObsMap(:,end-5:end)=0; % if obstacle is enabled in mask then add to occupancy map if strcmp(Arena.obj(idx).status,'on') obsLoc(1)=((Arena.obj(idx).position(1)+Arena.length/2)*mapScale)-(Arena.obj(idx).width*mapScale/2); obsLoc(2)=mapGridSize(1)-((Arena.obj(idx).position(2)+Arena.width/2)*mapScale)-(Arena.obj(idx).length*mapScale/2); obsLoc(3)= Arena.obj(idx).width*mapScale; obsLoc(4)= Arena.obj(idx).length*mapScale; % Get obstacle indices currObsLoc = ceil(obsLoc); colIdx = currObsLoc(1):(currObsLoc(1)+currObsLoc(3)); rowIdx = currObsLoc(2):(currObsLoc(2)+currObsLoc(4)); % Set obstacle indices to 0 in map locObsMap(rowIdx,colIdx) = 0; end end mapForSim.obsMap=locObsMap; mapForSim.scaleFactor=mapScale; % imshow(mapForSim.obsMap*240) % Create simulation map using binary occupancy grid % compMap = imcomplement(locObsMap); %binary occupancy grid flips this for some reason compMap=1-locObsMap; mapForSim.simMap = robotics.BinaryOccupancyGrid(compMap,mapScale); % image(mapForSim.obsMap*240) % show(mapForSim.simMap) % Only for testing % assignin('base','occGrid',mapForSim.simMap) end